/*
 * 础光实时操作系统PhotonRTOS -- 调度核心模块
 *
 * Copyright (C) 2022, 2023 国科础石(重庆)软件有限公司
 *
 * 作者: Baoyou Xie <xiebaoyou@kernelsoft.com>
 *
 * License terms: GNU General Public License (GPL) version 3
 *
 */

#include <photon/irq.h>
#include <photon/sched.h>
#include <photon/string.h>
#include <photon/bitops.h>
#include <photon/init_integrity.h>
#include <photon/smp.h>
#include <photon/smp_lock.h>
#include <photon/timex.h>
#include <autosar/internal.h>
#include <asm/process.h>
#include <asm/irq.h>
#include <asm/stack.h>
#include <asm/mpu.h>


#include <autosar/internal.h>

#define sched_runqueue_mask	(sched_runqueue_mask_table[smp_processor_id()])
#define sched_runqueue_list	(sched_runqueue_list_table[smp_processor_id()])
#define lock_all_task_list	(lock_all_task_list_table[smp_processor_id()])

#define THREAD_START_SP PROCESS_STACK_SIZE - 16 /* (PROCESS_STACK_SIZE - 16)	@ */

uint64_t sched_delay_time[MAX_CPUS];

#define cur_contex_switch_time sched_delay_time[smp_processor_id()]
/**
 * 主CPU的idle线程堆栈
 * 使用静态分配的目的，是为了提供给CPU初始化过程使用
 */
__attribute__((aligned(PROCESS_STACK_SIZE)))
union process_union master_idle_stack;
#if MAX_CPUS > 1
union process_union slave_idle_stack[MAX_CPUS - 1];
#endif

/**
 * 注意，是MAX_RT_PRIO + BITS_PER_LONG
 * 而不是MAX_RT_PRIO + BITS_PER_LONG **-1**
 * 这样最后总有一位是0
 */
#define SCHED_MASK_SIZE ((MAX_RT_PRIO + BITS_PER_LONG) / BITS_PER_LONG)
uintptr_t sched_runqueue_mask_table[MAX_CPUS][SCHED_MASK_SIZE] = { 0 };
struct double_list sched_runqueue_list_table[MAX_CPUS][MAX_RT_PRIO+1];

union process_union *idle_proc_stacks[MAX_CPUS];
struct task_desc idle_task_desc[MAX_CPUS];

struct smp_lock lock_all_task_list_table[MAX_CPUS];
struct double_list sched_all_task_list_table[MAX_CPUS];

static inline void add_to_runqueue(struct task_desc *p)
{
	int32_t	pri = p->sched_prio;

	if (p->in_run_queue != 0) {

		return;
	} else {

		list_insert_behind(&p->run_list, &(sched_runqueue_list[pri]));
		p->state = READY;
		p->in_run_queue = 1;
		if (p->sched_prio > current->sched_prio) {
			set_task_need_resched(current);
		}

		atomic_set_bit(pri, (uintptr_t *)sched_runqueue_mask);
	}
}

void del_from_runqueue(struct task_desc *p)
{
	int32_t pri = p->sched_prio;

	if (p->in_run_queue == 0) {
		return;
	} else {
		list_del(&p->run_list);
		list_init(&p->run_list);
		p->in_run_queue = 0;
		if (list_is_empty(&(sched_runqueue_list[pri])) != 0) {
			atomic_clear_bit(pri,
				(uintptr_t *)sched_runqueue_mask);
		}
	}
}

asmlinkage void preempt_schedule(void)
{
	if ((preempt_count() > 0U || irqs_disabled() > 0U)) {
		return;
	} else {
		do {
			/**
			* 如果不添加PREEMPT_ACTIVE标志，会出现什么情况?
			*/
			add_preempt_count(PREEMPT_ACTIVE);
			schedule();
			sub_preempt_count(PREEMPT_ACTIVE);

			barrier();
		} while (test_process_flag(PROCFLAG_NEED_RESCHED) != 0);
	}
}

asmlinkage void preempt_in_irq(void)
{
	
	if ((preempt_count() > 0U || irqs_disabled() == 0U)) {
		return;
	} else {
		do {
			add_preempt_count(PREEMPT_ACTIVE);

			/**
			* 这里不需要加内存屏障
			*/
			enable_irq();

			schedule();
			/**
			* 在退回到中断汇编处理前，这里应当关闭中断
			* 否则可能将堆栈击穿
			*/
			disable_irq();
			sub_preempt_count(PREEMPT_ACTIVE);

			barrier();
		} while (test_process_flag(PROCFLAG_NEED_RESCHED) != 0);
	}
}

static void __switch_to_context(struct process_desc *prev,
	struct process_desc *new)
{
	prev->task->jiffies64 = get_jiffies_64() - prev->task->jiffies64;
	new->task->jiffies64 = get_jiffies_64() - new->task->jiffies64;
#ifdef CONFIG_AUTOSAR_MEMORY_PROTECTION
	disable_memory_protection();
#endif
	arch_switch_cpu_context(prev->task, new->task);
#ifdef CONFIG_AUTOSAR_MEMORY_PROTECTION
	if (current->is_autosar_task) {
		arch_switch_mpu();
	}
#endif
}

asmlinkage void schedule(void)
{
	struct task_desc *prev, *next;
	uintptr_t flags;
	int32_t idx;

	if (irqs_disabled() || (preempt_count() & ~PREEMPT_ACTIVE)) {
		pr_err("cannt switch task, preempt count is %lx, irq %s.\n",
			preempt_count(),
			irqs_disabled() ? "disabled" : "enabled");
		BUG();
	}
#ifdef CONFIG_AUTOSAR_STACK_MONITOR
	if (time_protect_disable == 0 && current->is_autosar_task &&
		current_proc_info()->magic != TASK_MAGIC) {
			autosar_protect_hook(E_OS_STACKFAULT);
		}
#endif
need_resched:
	/**
	 * 这里必须手动增加抢占计数
	 * 以避免在打开锁的时候执行调度，那样就乱套了
	 */
	preempt_disable();
	smp_lock_irqsave(&lock_all_task_list, flags);

	prev = current;
	/**
	 * 很微妙的两个标志，请特别小心
	 */
	if (!(preempt_count() & PREEMPT_ACTIVE) && prev->state != RUNNING) {
		del_from_runqueue(prev);
#ifdef AUTOSAR_TIMING_PROTECTION
		if (prev->is_autosar_task) {
			prev->cp_attr.cp_task_execution_time = 0;
			prev->cp_attr.cp_task_start_point = 0;
		}
#endif
	}
	idx = find_last_bit(sched_runqueue_mask, MAX_RT_PRIO + 1);

	/**
	 * 没有任务可运行
	 */
	if (idx > MAX_RT_PRIO) {

		/**
		 * 选择本CPU上的IDLE任务来运行
		 */
		next = &idle_task_desc[smp_processor_id()];
	} else {

		next = list_first_container(&sched_runqueue_list[idx], struct task_desc, run_list);
		if (current->sched_policy == SCHED_TIMESLICE && current == next) {
			del_from_runqueue(next);
			add_to_runqueue(next);
			next = list_first_container(&sched_runqueue_list[idx], struct task_desc, run_list);
		}
	}

	/**
	 * 什么情况下，二者会相等??
	 */
	if (unlikely(prev == next)) {
		clear_task_need_resched(prev);

		prev->state = RUNNING;
		smp_unlock_irq(&lock_all_task_list);
		preempt_enable_no_resched();
		goto same_process;
	}
	clear_task_need_resched(prev);

	next->prev_sched = prev;
	task_process_info(next)->cpu = task_process_info(prev)->cpu;
	if (prev->is_autosar_task && (prev->state == RUNNING)) {
		prev->state = READY;
#ifdef AUTOSAR_TIMING_PROTECTION
		if (prev->cp_attr.cp_task_start_point != 0) {
			prev->cp_attr.cp_task_execution_time +=
			cycles_to_usecs(get_cycles()) - prev->cp_attr.cp_task_start_point;
			if (time_protect_disable == 0 &&
				prev->cp_attr.cp_task_execution_budget < prev->cp_attr.cp_task_execution_time && time_protect_disable == 0) {
					autosar_protect_hook(E_OS_PROTECTION_TIME);
			}
		}
		prev->cp_attr.cp_task_start_point = cycles_to_usecs(get_cycles());
#endif
	}
	if (next->is_autosar_task && next->state == READY) {
#ifdef AUTOSAR_TIMING_PROTECTION
		next->cp_attr.cp_task_start_point = cycles_to_usecs(get_cycles());
#endif
	}
	cur_contex_switch_time = cycles_to_usecs(get_cycles());
	__switch_to_context(task_process_info(prev), task_process_info(next));
	current->live_tick = 10;
	current->state = RUNNING;

	cur_contex_switch_time = cycles_to_usecs(get_cycles()) - cur_contex_switch_time;
	barrier();

	smp_unlock_irq(&lock_all_task_list);
	preempt_enable_no_resched();

same_process:
	if (unlikely(test_process_flag(PROCFLAG_NEED_RESCHED))) {
		goto need_resched;
	}
}

static void exit_task_check(void)
{
	/**
	 * [SWS_Os_00052] ⌈如果任务从其入口函数返回而没有进行 TerminateTask()或 ChainTask()调用，
	 * 操作系统模块将终止该任务（如果已配置 PostTaskHook，则调用 PostTaskHook() ）。 ⌋ ( SRS_Os_11009 )
	 */
#if defined(CONFIG_OSEK_HOOK_POST_SCHEDULE)
	CONTEXT_MARK_START(ENV_POSTTASK_HOOK)
	PostTaskHook();
	CONTEXT_MARK_END
#endif

	/**
	 * [SWS_Os_00239] ⌈如果任务从入口函数返回而没有进行 TerminateTask()或 ChainTask()
	 * 调用并且中断仍然被禁用，操作系统模块应启用它们。 ⌋ ( )
	 */
	osek_interrupt_restore();

	/**
	 * [SWS_Os_00070] ⌈如果任务从入口函数返回而没有调用 TerminateTask()或 ChainTask()
	 * 并且仍然持有 OSEK 资源，操作系统模块将释放它们。 ⌋ ( SRS_Os_11009 , SRS_Os_11013 )
	 */
	osek_release_all_resource();

	/**
	 * [SWS_Os_00069] ⌈如果一个任务从它的入口函数返回而没有创建一个 TerminateTask()或 ChainTask()
	 * 调用并配置了错误钩子，操作系统模块应在任务离开 RUNNING 状态之前调用状态为 E_OS_MISSINGEND 的
	 *  ErrorHook() （无论任务是否导致其他错误，例如 E_OS_RESOURCE ）。 ⌋ ( SRS_Os_11009 )
	 */
	osek_call_hook_0(TerminateTask, E_OS_MISSINGEND);
}
extern char __app0_data_end[];
/**
 * 任务最初的入口
 */
static void task_entry(void)
{
#ifdef CONFIG_AUTOSAR_MEMORY_PROTECTION
	arch_switch_mpu();
#endif

	struct task_desc *tsk = current;
	union process_union *stack = (union process_union *)current_proc_info();
	void (*func)(void) = NULL;
	func = tsk->entry;

	stack->process_desc.preempt_count = 2;
	tsk->state = RUNNING;
	/**
	 * 稍微有点费解
	 * 这里是与schedule函数前半部分对应
	 */
	smp_unlock(&lock_all_task_list);
	enable_irq();
	preempt_enable();

	/**
	 * 执行任务主函数
	 */
	func();

	/**
	 * 任务直接返回，而没有调用OSEK的结束功能
	 */
	exit_task_check();

	sched_deactivate_task(tsk->osek_id);

	/**
	 * 调用schedule之前必须保证是打开中断的
	 */
	enable_irq();
	
	/**
	 * 任务退出后，调度出去，不能继续运行
	 */
	schedule();

	panic("TASK exit without TerminateTask()\n");
}

static void __init_task(union process_union *stack, struct task_desc *proc)
{
	proc->magic = TASK_MAGIC;
	proc->state = SUSPENDED;
	proc->sched_prio = proc->prio;
	proc->in_run_queue = 0;
	proc->prev_sched = NULL;
	proc->osek_id = INVALID_OSEK_ID;
	proc->stack_size = PROCESS_STACK_SIZE;

	stack->process_desc.preempt_count = 0;

	list_init(&proc->run_list);
	list_init(&proc->all_list);
}


static void init_idle_process(union process_union *stack,
		struct task_desc *proc, int32_t cpu)
{
	if (stack == NULL || proc == NULL) {
		BUG();
	}

	memset(proc, 0, sizeof(*proc));
	stack->process_desc.task = proc;
	stack->process_desc.cpu = cpu;
	stack->process_desc.magic = TASK_MAGIC;
	stack->process_desc.object.app_id = INVALID_OSAPPLICATION;
	stack->process_desc.object.trusted_func_app_id = INVALID_OSAPPLICATION;
	proc->stack = stack->stack;
	proc->core_id = cpu;

	proc->task_main = &cpu_idle;
	proc->flags = 0;
	__init_task(stack, proc);
	proc->state = RUNNING;
}


/**
 * 调度模块早期初始化
 * 主要是初始化所有核上面的idle堆栈
 */
void init_sched_early(void)
{
	union process_union *stack;
	struct task_desc *proc;
	int32_t i;

	MODULE_INIT(init_sched_early);
	for (i = 0; i < MAX_CPUS; i++) {
		if (i == 0) {
			idle_proc_stacks[i] = &master_idle_stack;
#if MAX_CPUS > 1
		} else {
			idle_proc_stacks[i] = &slave_idle_stack[i - 1];
#endif
		}
		stack = idle_proc_stacks[i];
		proc = &idle_task_desc[i];
		init_idle_process(stack, proc, i);
	}
}

/**
 * 从静态数组中初始化任务
 */
void sched_init_task(uint32_t id)
{
	union process_union *stack;
	struct task_desc *proc;

	if (id >=OS_NR_TASK) {
		BUG();
	}

	stack = &osek_stacks[id];
	proc = &osek_tasks[id];
	if (stack == NULL) {
		BUG();
	}

	stack->process_desc.task = proc;
	stack->process_desc.cpu = proc->core_id;
	stack->process_desc.magic = TASK_MAGIC;
	stack->process_desc.object.object_type = TYPE_TASK;
	stack->process_desc.object.trusted_func_app_id = INVALID_OSAPPLICATION;
	proc->stack = stack;
	proc->task_main = proc->entry;
	__init_task(stack, proc);
	/**
	 * 注意在__init_task之后设置
	 */
	proc->osek_id = id;
	proc->state = SUSPENDED;
	proc->max_activate_count = 8;
}

/**
 * 从静态数组中激活任务
 */
void sched_activate_task(int32_t id)
{
	union process_union *stack;
	struct task_desc *tsk;
	uintptr_t flags;

	if (id < 0 || id >=OS_NR_TASK) {
		BUG();
	}
	stack = &osek_stacks[id];
	tsk = &osek_tasks[id];

	tsk->live_tick = 10;
	tsk->prev_sched = NULL;
    /* Unlink next two context's */
	arch_task_init(stack, task_entry, tsk);

	list_init(&tsk->run_list);
	list_init(&tsk->all_list);
	smp_lock_irqsave(&lock_all_task_list, flags);
	add_to_runqueue(tsk);
	list_insert_behind(&tsk->all_list, &sched_all_task_list);
	smp_unlock_irqrestore(&lock_all_task_list, flags);
}

/**
 * 从静态数组中去激活任务
 */
void sched_deactivate_task(int32_t id)
{
	struct task_desc *tsk;
	uintptr_t flags;

	if (id < 0 || id >=OS_NR_TASK) {
		BUG();
	}

	tsk = &osek_tasks[id];

	smp_lock_irqsave(&lock_all_task_list, flags);
	tsk->state = SUSPENDED;
	del_from_runqueue(tsk);
	smp_unlock_irqrestore(&lock_all_task_list, flags);

}

/**
 * 调整任务优先级
 */
void sched_task_change_prio(TaskType task_id, uint32_t prio)
{
	struct task_desc *tsk;
	uint32_t idx;
	uintptr_t flags;

	if (task_id >= (TaskType)OS_NR_TASK) {
		BUG();
	}

	tsk = &osek_tasks[task_id];
	if (tsk->sched_prio == prio) {
		return;
	}

	smp_lock_irqsave(&lock_all_task_list, flags);

	if (tsk->in_run_queue == 0) {//如果task本来就不在run_queue中
		tsk->sched_prio = prio;
	} else {
		del_from_runqueue(tsk);
		tsk->sched_prio = prio;
		if (current == tsk) {
			idx = find_last_bit(sched_runqueue_mask, MAX_RT_PRIO + 1);
			if (idx > prio) {//如果是降低自己的优先级
				set_task_need_resched(tsk);
			}
			list_insert_behind(&tsk->run_list, &(sched_runqueue_list[prio]));
			atomic_set_bit(prio, (uintptr_t *)sched_runqueue_mask);
			tsk->in_run_queue = 1;
		} else {
			add_to_runqueue(tsk);
		}
	}

	smp_unlock_irqrestore(&lock_all_task_list, flags);
}

/**
 * yield调用
 */
void sched_yield(void)
{
	schedule();
}

/**
 * 唤醒OSEK任务
 */
void sched_task_wakeup(TaskType task_id)
{
	struct task_desc *tsk;
	uintptr_t flags;

	if (task_id >= (TaskType)OS_NR_TASK) {
		BUG();
	}

	tsk = &osek_tasks[task_id];

	smp_lock_irqsave(&lock_all_task_list, flags);
	add_to_runqueue(tsk);
	smp_unlock_irqrestore(&lock_all_task_list, flags);
}

/**
 * 调度初始化函数
 */
void init_sched(void)
{
	int32_t i, j;

	MODULE_INIT(init_sched);
	for (i = 0; i < ARRAY_SIZE(sched_all_task_list_table); i++) {
		list_init(&sched_all_task_list_table[i]);
		smp_lock_init(&lock_all_task_list_table[i]);
	}

	if (!irqs_disabled()) {
		pr_err("！请在此之前关闭中断！");
		disable_irq();
	}

	for (j = 0; j < MAX_CPUS; j++) {
		for (i = 0; i < ARRAY_SIZE(sched_runqueue_list_table[j]); i++)
			list_init(&sched_runqueue_list_table[j][i]);
		memset(sched_runqueue_mask_table[j], 0, sizeof(sched_runqueue_mask_table[j]));
	}
}
